package crtl;


/**
 *
 * @author meraner marcel
 */
public class ProportionalBalanceBallCtrl extends ProportionalCtrl {

	private final int _minDSensorValue = 100;
	
	private final double[][] _controllerMatrix = {
		{1, 1, 1, 1, 0, 0, 0, 0},
		{0, 0, 0, 0, 1, 1, 1, 1}
	};
	private final double[] _constants = {0.1, 0.1};
	
	public ProportionalBalanceBallCtrl() {
		super();

		System.out.println("Proportional - Balance Ball Controller initialized");
	}
	
	
	@Override
	protected boolean finished() {
		return false;
	}

	@Override
	protected double[] getConstants() {
		return _constants;
	}

	@Override
	protected double[][] getControllMatrix() {
		return _controllerMatrix;
	}

	@Override
	protected double[] fetchAndPrepareSensorValues() {
		double[] s = readDistanceSensors();
		s = calcAverage(s);
		return normalize(s, _minDSensorValue, findMax(s));
	}
	
}
